#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "heap.cuh"

#define PI 3.14159


float heapGauss(float mean, float stddev)
{
	/*Box-Mueller filter;*/
	float u = (float)(rand() / (1.0*RAND_MAX));
	float v = (float)(rand() / (1.0*RAND_MAX));
	return mean + (float)(sqrt(-2*log(u))*cos(2*PI*v)*stddev);
}  

void  heapElementInit(heapElement * ELEMENT, int id){
	int i;

	/*Danger!*/
	ELEMENT -> address = id;
	ELEMENT -> fitness = 0;
	ELEMENT -> oldfitness = 0;
	ELEMENT -> mutCount=0;
	ELEMENT -> mutEvents=0;
	ELEMENT -> mutSuccess=0;
	ELEMENT -> mutCum=0;
	ELEMENT -> mutClone=1;



	//float iv[16] = {2.168713,0.825571,4.061320,0.602497,400.858082,0.925744,0.118088,15.013704,3.541623,0.257316,5.157650,0.879355,29.813459,1.052426,0.519770,1.0};
	//for (i=0; i<VECTORSIZE; i++) ELEMENT->vector[i] = iv[i] + abs(heapGauss(0.0,1.0)); 

	for (i=0; i<VECTORSIZE; i++) ELEMENT->vector[i] = abs(heapGauss(0.0,1.0)); 
	//for (i=0; i<VECTORSIZE; i++) ELEMENT->vector[i] = 1.0; 
	
	
	//Last two factors to determine final level
	//ELEMENT->vector[VECTORSIZE-1] = 0.001;
	//ELEMENT->vector[VECTORSIZE-2] = 0.001;
	
	
	/*
	Test init.
	ELEMENT ->vector[0] = 0.512976;
	ELEMENT ->vector[1] = 4.184026;
	ELEMENT ->vector[2] = abs(heapGauss(5.0,SIGMA));
	ELEMENT ->vector[3] = abs(heapGauss(6.0,SIGMA));
	ELEMENT ->vector[4] = 0.580024;
	ELEMENT ->vector[5] = 2.581272;
	*/

	//WARNING
	//ELEMENT->vector[14] = 1.0+abs(heapGauss(0.0,1.0));
	
}

void heapLoad(heap * HEAP){
	#include LOADPATH
}

void heapSave(heap * HEAP, int n){
	int i;
	int j;
	char * dst;
	char * buf;
	FILE * fps;

	dst = (char *)malloc(sizeof(char)*256);
	buf = (char *)malloc(sizeof(char)*256);

	sprintf(dst, SAVEPATH, n);
	fps = fopen(dst, "w");
	
	for(i=0; i<=HEAPSIZE; i++){
		sprintf(buf, "HEAP->elementArray[%d]->fitness = %f; ", i, HEAP->elementArray[i]->fitness); fprintf(fps,buf);
		sprintf(buf, "HEAP->elementArray[%d]->oldfitness = %f; ", i, HEAP->elementArray[i]->oldfitness); fprintf(fps,buf);
		sprintf(buf, "HEAP->elementArray[%d]->address = %d; ", i, HEAP->elementArray[i]->address); fprintf(fps,buf);
		sprintf(buf, "HEAP->elementArray[%d]->mutCount = 0; ", i); fprintf(fps,buf);
		sprintf(buf, "HEAP->elementArray[%d]->mutSuccess = 0; ", i); fprintf(fps,buf);
		sprintf(buf, "HEAP->elementArray[%d]->mutClone = 0; ", i); fprintf(fps,buf);
		for(j=0; j<VECTORSIZE; j++){
			sprintf(buf, "HEAP->elementArray[%d]->vector[%d] = %f; ", i, j, HEAP->elementArray[i]->vector[j]); fprintf(fps,buf);
		}
	fprintf(fps, "\n");	
	}
	fclose(fps);
}



void heapMutateMoga(heap * HEAP){

  int i;
  int j;
  int k;
  int f;
  float a;
  float csum;

  //Empty elements;
  for(j=0;j<VECTORSIZE; j++){
	  HEAP->vectorMeans[j] = 0;
	  HEAP->vectorSigma[j] = 0;
  }


  //Determine cumulative probability
  for(i=1; i<=HEAPSIZE;i++){
    f = 0;
    for(j=1; j<=HEAPSIZE;j++){
      if(HEAP->elementArray[i]->fitness < HEAP->elementArray[j]->fitness) f++;
    }
    HEAP->elementArray[i]->mutCum = f;
    HEAP->elementArray[i]->mutEvents = (int)(VECTORSIZE*(1.0-(((float)f)/HEAPSIZE)))+1;
    if(HEAP->elementArray[i]->mutEvents > VECTORSIZE){
      HEAP->elementArray[i]->mutEvents = VECTORSIZE;
    }
  }

  //Mean fitness
  csum = 0;
  for(i=1;i<=HEAPSIZE; i++){
    for(j=0;j<VECTORSIZE; j++) HEAP->vectorMeans[j] = HEAP->vectorMeans[j] +HEAP->elementArray[i]->vector[j];
    csum = csum+HEAP->elementArray[i]->mutCum;
  } 
  for(j=0;j<VECTORSIZE; j++) {HEAP->vectorMeans[j] = HEAP->vectorMeans[j]/HEAPSIZE; fprintf(fp,"%f ", HEAP->vectorMeans[j]);}
  fprintf(fp,". (means vector)\n");

  //determine Sigmas
  for(j=0; j<VECTORSIZE; j++){
    a = 0;
    for(i=1;i<=HEAPSIZE; i++) a = a+pow((HEAP->elementArray[i]->vector[j] - HEAP->vectorMeans[j]),2) * HEAP->elementArray[i]->mutCum;
    a = sqrt(a/csum);
    HEAP -> vectorSigma[j] = a;
    fprintf(fp,"%f ", HEAP -> vectorSigma[j]);
  }
  fprintf(fp,". (sigma vector)\n");


  //Sort indexes
  float smax;
  int sidx;
  for(j=0; j<VECTORSIZE; j++){
      smax = 0;
      sidx = 0;
      for(k=0; k<VECTORSIZE; k++){
        if(HEAP->vectorSigma[k] > smax){
          smax = HEAP->vectorSigma[k];
          sidx = k;
        }
      }
      HEAP->vectorSigmaIdx[j] = sidx;
      HEAP->vectorSigma[sidx] *= -1;
    //fprintf(fp,"%d ", HEAP -> vectorSigmaIdx[j]);
  }
  //fprintf(fp,". (sigma index vector)\n");


  //Mutate population
  for(i=2;i<=HEAPSIZE; i++){
    //fprintf(fp,"\t Mutating element with fitness %f, %d times, CF:%d.  \n",HEAP->elementArray[i]->fitness,HEAP->elementArray[i]->mutEvents,HEAP->elementArray[i]->mutCum);
    
	  
	for(j=0;j<HEAP->elementArray[i]->mutEvents; j++){
      k = HEAP -> vectorSigmaIdx[j];
      //fprintf(fp, "\t\t %d-th max is %f\n", j, HEAP->vectorSigma[k]);
        //fprintf(fp, "\t\t\t old: %f", HEAP->elementArray[i]->vector[k]);
        //HEAP->elementArray[i]->vector[k] += heapGauss(MI,SIGMA);
		
		//Last two treated differently
		//if (k>=VECTORSIZE-2) HEAP->elementArray[i]->vector[k] *= heapGauss(MI,SIGMA);
	  
	  //if (k>=VECTORSIZE-2) HEAP->elementArray[i]->vector[k] *= 1.0;
		//else HEAP->elementArray[i]->vector[k] = heapGauss(MI,SIGMA);
		//else HEAP->elementArray[i]->vector[k] *= heapGauss(MI,SIGMA);
		  
		//HEAP->elementArray[i]->vector[k] *= heapGauss(MI,SIGMA);
		HEAP->elementArray[i]->vector[k] += heapGauss(MI,SIGMA);


		//HEAP->elementArray[i]->vector[k] = heapGauss(MI,SIGMA);
        if(HEAP->elementArray[i]->vector[k] < 0.0f) HEAP->elementArray[i]->vector[k] *= -1.0;
		//fprintf(fp, "\t\t\t new: %f \n", HEAP->elementArray[i]->vector[k]);

		//KRAB RIGHT OF WAY
		//if(HEAP->elementArray[i]->vector[14] <= 0.5f) HEAP->elementArray[i]->vector[14] = 0.5 + abs(heapGauss(MI,SIGMA));
        
    }

  }
  



}

void heapElementMutate(heapElement * ELEMENT){
	int i;

	/*Reset counter.*/
	ELEMENT -> mutCount=0;
	
	/*TODO: keep above zero;*/
	for (i=0; i<VECTORSIZE; i++){
		if(((1.0*rand())/RAND_MAX) < MUTATION){
			//fprintf(fp,"[%X]-Mutation-%d ",ELEMENT->address,i); 
			ELEMENT->vector[i] = ELEMENT->vector[i] * heapGauss(0.0,SIGMA);
			ELEMENT->mutCount++;
			MUTATIONCOUNT++;
			heapMutationControl();
		}
		if(ELEMENT->vector[i] < 0) ELEMENT->vector[i] *= -1;
	}
	//fprintf(fp,"\n");
	ELEMENT->mutClone=1;
}

void heapMutationControl(){
	if (MUTATIONCOUNT == MFACTOR*VECTORSIZE){
		if ((1.0*MUTATIONSUCCESS)/MUTATIONCOUNT < 0.2f) SIGMA *= 1.0f/MDELTA;
		else if ((1.0*MUTATIONSUCCESS)/MUTATIONCOUNT > 0.2f) SIGMA *= MDELTA;
		MUTATIONCOUNT = 0;
		MUTATIONSUCCESS = 0;
		fprintf(fp, "\n Mutation control SIGMA:%d \n!", SIGMA);
	}

}


void heapElementCrossover(heapElement * DST, heapElement * E1, heapElement * E2){
  /*Single-point crossover*/
	int c;
	int i;

	c = 1+(int)((rand() / (1.0*RAND_MAX))*VECTORSIZE);
	//fprintf(fp,"\t e1:%X e2:%X c:%d \n", E1->address, E2->address, c);
	for(i=0; i<VECTORSIZE; i++){
		if (i<c) DST->vector[i] = E1->vector[i];
		else DST->vector[i] = E2->vector[i];
	}
	DST->mutClone=1;
}

void heapElementCrossoverUni(heapElement * DST, heapElement * E1, heapElement * E2){
  /*N-point crossover*/
  int i;

  //fprintf(fp,"\t \t e1:%X e2:%X \n", E1->address, E2->address);
	for(i=0; i<VECTORSIZE; i++){
  	if ( 1.0*rand()/RAND_MAX < 0.5 ) DST->vector[i] = E1->vector[i];
		else DST->vector[i] = E2->vector[i];
	}
	DST->mutClone=1;
}

void heapElementCrossoverAsex(heapElement * DST, heapElement * E1, heapElement * E2){
  /*Asexual reproduction*/
  int i;

	for(i=0; i<VECTORSIZE; i++){
		DST->vector[i] = E1->vector[i];
	}
	DST->mutClone=1;
}


void heapIfy(heap * HEAP, int i, int size){
	int left;
	int right;
	int smallest;
	heapElement * tempEL;

	left	= 2*i;
	right	= 2*i+1; 
	
	if (left <= size && HEAP->elementArray[left] -> fitness < HEAP->elementArray[i]  -> fitness) smallest = left;
	else smallest = i;
	if (right <= size && HEAP-> elementArray[right]  -> fitness < HEAP->elementArray[smallest] -> fitness) smallest = right;
	if (smallest != i){
		tempEL = HEAP->elementArray[i];
		HEAP->elementArray[i] = HEAP->elementArray[smallest];
		HEAP->elementArray[smallest] = tempEL; 
		heapIfy(HEAP, smallest,size);
	}
	
};

void heapIfyMax(heap * HEAP, int i, int size){
	int left;
	int right;
	int largest;
	heapElement * tempEL;

	left	= 2*i;
	right	= 2*i+1; 
	
	if (left <= size && HEAP->elementArray[left] -> fitness > HEAP->elementArray[i] -> fitness) largest = left;
	else largest = i;
	if (right <= size && HEAP-> elementArray[right] -> fitness > HEAP->elementArray[largest] -> fitness) largest = right;
	if (largest != i){
		tempEL = HEAP->elementArray[i];
		HEAP->elementArray[i] = HEAP->elementArray[largest];
		HEAP->elementArray[largest] = tempEL; 
		heapIfyMax(HEAP, largest,size);
	}
	
};

void heapInsert(heap * HEAP, int i, heapElement * element){
	HEAP -> elementArray[i] = element;
};


void heapBuild(heap * HEAP){
	int i;
	for (i=HEAPSIZE/2;i>0;i--) heapIfy(HEAP,i,HEAPSIZE);
}

void heapBuildMax(heap * HEAP){
	int i;
	for (i=HEAPSIZE/2;i>0;i--) heapIfyMax(HEAP,i,HEAPSIZE);
}

void heapSort(heap * HEAP){
	int i;
	int size;
	heapElement * tempEL;

	heapBuildMax(HEAP);
	size = HEAPSIZE;
	for (i=HEAPSIZE; i>1; i--){
		tempEL = HEAP->elementArray[i];
		HEAP->elementArray[i] = HEAP->elementArray[1];
		HEAP->elementArray[1] = tempEL; 
		size--;
		heapIfyMax(HEAP,1,size);
	}

}

void heapPrintElement(heapElement * ELEMENT){
	
	
	int i;
	fprintf(fp,"%.3f Address: [%X] old-fitness:(%.3f) events:(%.3f)",	ELEMENT->fitness, ELEMENT->address, ELEMENT->oldfitness, ELEMENT->mutEvents);
	fprintf(fp,"vector: "); for(i=0; i<VECTORSIZE; i++) fprintf(fp," %f, ", ELEMENT->vector[i]); fprintf(fp,"\n");
	
	
	/*	
	fprintf(fp,"statebfp: "); for(i=0; i<HISTSIZE; i++){fprintf(fp," %.1f ",ELEMENT->statebfp[IND_0][i]);}; fprintf(fp,"\n");
	fprintf(fp,"statemct: "); for(i=0; i<HISTSIZE; i++){fprintf(fp," %.1f ",ELEMENT->statemct[IND_0][i]);}; fprintf(fp,"\n");
	fprintf(fp,"statebfpPC: "); for(i=0; i<HISTSIZE; i++){fprintf(fp," %d ",ELEMENT->statebfp[IND_PC][i]);}; fprintf(fp,"\n");
	fprintf(fp,"statemctPC: "); for(i=0; i<HISTSIZE; i++){fprintf(fp," %d ",ELEMENT->statemct[IND_PC][i]);}; fprintf(fp,"\n");
	fprintf(fp,"statebfpEC: "); for(i=0; i<HISTSIZE; i++){fprintf(fp," %d ",ELEMENT->statebfp[IND_EC][i]);}; fprintf(fp,"\n");
	fprintf(fp,"statemctEC: "); for(i=0; i<HISTSIZE; i++){fprintf(fp," %d ",ELEMENT->statemct[IND_EC][i]);}; fprintf(fp,"\n");	
	*/	
	fprintf(fp,"\n");
}

void heapPrint(heap * HEAP){
	int i;
	for(i=HEAPSIZE; i>=2;i--){ fprintf(fp,"x "); heapPrintElement(HEAP->elementArray[i]);}
	fprintf(fp,"= "); heapPrintElement(HEAP->elementArray[1]);

}


void heapElementFitness(heapElement * ELEMENT){
	
	//Misc
	int i;
	float bf = 0;
	float mf = 0;
	
	//Store previous value;
	ELEMENT->oldfitness = ELEMENT->fitness;
	ELEMENT->mutSuccess = 0;

	for(i=0; i<HISTSIZE; i++){
		bf += (ELEMENT->errorbfp[i])/5.0;
		mf += (ELEMENT->errormct[i])/5.0;
	}
	bf = bf/HISTSIZE;
	mf = mf/HISTSIZE;

	//Take greater error.
	//ELEMENT->fitness = (bf < mf)*mf*ERRF + (bf >= mf)*bf*ERRF;

	//Take average error
	ELEMENT->fitness = (ERRF*bf+ERRF*mf)/2.0;

	//Inverse geometric
	//ELEMENT->fitness = pow((float)(bf*mf),2.0f);

	//Increase global mutation count
	if(ELEMENT->mutCount > 0 && ELEMENT->fitness < ELEMENT->oldfitness){
		MUTATIONSUCCESS += ELEMENT->mutCount;
	}
	ELEMENT -> mutClone = 0;	


  
}
